/*
 *  quickhull.cpp
 *  bgsubtraction
 *
 *  Created by a1gucis on 5/14/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "quickhull.h"

int QuickHull::eucDist(int row1, int col1, int row2, int col2) {
	int dist = (row2-row1)*(row2-row1)+(col2-col1)*(col2-col1);
	return dist;
}

void QuickHull::divide(vector<int> **rowVector, vector<int> **columnVector, int row1, int col1, int row2, int col2, bool right, int &maxRow, int &maxCol) {
	vector<int> *newRowVector = new vector<int>();
	vector<int> *newColVector = new vector<int>();
	float A, B, a, b, c, p, M, f, m, n, cosA, sinA;
	bool recalculate = false;
	if ((col1 - col2) != 0) {
		recalculate = true;
		//one way to represent a line
		m = float(row1 - row2)/float(col1 - col2);
		n = (float)row2 - m*float(col2);
		
		//another way to represent a line
		A = col2 - col1;
		B = row2 - row1;
		//cout<<A<<" "<<B<<"AB"<<endl;
		a = (-1)*B; //calculating the line normal
		b = A;
		
		c = (-1)*a*(float)col1 - b*(float)row1;
		M = 1.0/sqrt(a*a+b*b);
		if ((c < 0) && (M < 0))
			M *= -1;
		if ((c > 0) && (M > 0))
			M *= -1;
		//cout<<"M: "<<M<<" c:"<<c<<endl;
		p = (-1)*M*c;
		if (p < 0)
			p *= -1;
		cosA = M*a;
		sinA = M*b;
	}
	else {
		f = col1;
	}
	float maxDist = 0.0;
	//float maxRow = 0;
	//float maxCol = 0;
	for (int i=0;i<(*rowVector)->size();i++) {
		int row = (**rowVector)[i];
		int col = (**columnVector)[i];
		float d;
		if (recalculate) {
			f = m*(float)col + n;
			d = col*cosA + row*sinA - p;
			if (d < 0)
				d *= -1;
			//cout<<row<<" "<<col<<": "<<d<<endl;
		} else {
			d = eucDist(row, f, row, col);
		}
		if (row < round(f) && right && d > 0) {
			newRowVector->push_back(row);
			newColVector->push_back(col);
			if (d > maxDist) {
				maxDist = d;
				maxRow = row;
				maxCol = col;
			}
			
		} 
		if (row > round(f) && !right && d > 0){
			newRowVector->push_back(row);
			newColVector->push_back(col);
			if (d > maxDist) {
				maxDist = d;
				maxRow = row;
				maxCol = col;
			}
			
		}
	}
	(*rowVector)->clear();
	(*columnVector)->clear();
	delete *rowVector;
	delete *columnVector;
	*rowVector = newRowVector;
	*columnVector = newColVector;
}

void QuickHull::findHull(vector<int> **rowVector, vector<int> **columnVector, vector<ContourPixel *> *hullPixels, int currentIteration, int totalIterations, int row1, int col1, int row2, int col2, bool right) {
	int maxRow, maxCol;
	divide(rowVector, columnVector, row1, col1, row2, col2, right, maxRow, maxCol);
	if (((*rowVector)->size() == 0) || (currentIteration == totalIterations))
		return;
	vector<int> *rowVectorCopy = new vector<int>((*rowVector)->size());
	vector<int> *columnVectorCopy = new vector<int>((*rowVector)->size());
	cout<<"After division "<<row1<<" "<<col1<<" "<<row2<<" "<<col2;
	if (right) cout<<" right "<<endl;
	else cout<<" left"<<endl;
	for (int i=0;i<(*rowVector)->size();i++) {
		(*rowVectorCopy)[i] = (**rowVector)[i];
		(*columnVectorCopy)[i] = (**columnVector)[i];
		cout<<"r:"<<(**rowVector)[i]<<",c:"<<(**columnVector)[i]<<endl;
	}
	ContourPixel *contourPixel = new ContourPixel(maxRow, maxCol);
	cout<<"maxRow: "<<maxRow<<" maxCol: "<<maxCol<<endl;
	hullPixels->push_back(contourPixel);
	findHull(rowVector, columnVector, hullPixels, currentIteration++, totalIterations, row1, col1, maxRow, maxCol, right);
	//cout<<"rowVector size: "<<rowVector->size()<<endl;;
	findHull(&rowVectorCopy, &columnVectorCopy, hullPixels, currentIteration++, totalIterations, row1, col1, maxRow, maxCol, !right);
}
/*void QuickHull::findHull(vector<int> *rowVector, vector<int> *columnVector, vector<int> *rightRows, vector<int> *rightCols, vector<ContourPixel *> *hullPixels, int row1, int col1, int row2, int col2) {
	if (rowVector->size() == 0)
		return;
	int maxEucDist = 0;
	int maxEucPos = 0;
	for (int i=0;i<rowVector->size();i++) {
		int dist1 = eucDist((*rowVector)[row1], (*columnVector)[col1], (*rowVector)[i], (*columnVector)[i]);
		int dist2 = eucDist((*rowVector)[row2], (*columnVector)[col2], (*rowVector)[i], (*columnVector)[i]);
		int dist = dist1 + dist2;
		if (dist > maxEucDist) {
			maxEucDist = dist;
			maxEucPos = i;
		}
	}
	ContourPixel *contourPixel = new ContourPixel((*rowVector)[maxEucPos], (*columnVector)[maxEucPos]);
	int maxRow = (*rowVector)[maxEucPos];
	int maxCol = (*columnVector)[maxEucPos];
	hullPixels->push_back(contourPixel);
	
	divide(rowVector, columnVector, rightRows, rightCols, row1, col1, (*rowVector)[maxEucPos], (*columnVector)[maxEucPos], true);
	//rowVector->clear();
	//columnVector->clear();
	vector<int> *newRightRows = new vector<int>();
	vector<int> *newRightCols = new vector<int>();
	findHull(rightRows, rightCols, newRightRows, newRightCols, hullPixels, row1, col1, maxRow, maxCol);
	divide(rowVector, columnVector, rightRows, rightCols, row1, col1,  (*rowVector)[maxEucPos], (*columnVector)[maxEucPos], false);
	
	newRightRows = new vector<int>();
	newRightCols = new vector<int>();
	findHull(rightRows, rightCols, newRightRows, newRightCols, hullPixels, row2, col2, maxRow, maxCol);
}*/

vector<ContourPixel *> *QuickHull::contourPixels(vector<int> *rowVector, vector<int> *columnVector, int topRow, int leftCol, int bottomRow, int rightCol, int totalIterations) {
	int recursiveIterations = 0;
	int row1 = topRow;
	int col1 = leftCol;
	int row2 = bottomRow;
	int col2 = rightCol;
	vector<ContourPixel *> *hullPixels = new vector<ContourPixel *>();
	ContourPixel *hullPix = new ContourPixel(row1, col1);
	hullPixels->push_back(hullPix);
	hullPix = new ContourPixel(row2, col2);
	hullPixels->push_back(hullPix);
	int currentIteration = 0;
	vector<int> *rowVectorCopy = new vector<int>(rowVector->size());
	vector<int> *columnVectorCopy = new vector<int>(columnVector->size());
	for (int i=0;i<rowVector->size();i++) {
		(*rowVectorCopy)[i] = (*rowVector)[i];
		(*columnVectorCopy)[i] = (*columnVector)[i];
	}
	//for (int i=0;i<totalIterations;i++) {
	//	findHull(&rowVector, &columnVector, hullPixels, 0, totalIterations, row1, col1, row2, col2, true, maxRow, maxCol);
	//	
	//}
	findHull(&rowVector, &columnVector, hullPixels, currentIteration, totalIterations, row1, col1, row2, col2, true);
	findHull(&rowVectorCopy, &columnVectorCopy, hullPixels, currentIteration, totalIterations, row2, col2, row1, col1, false);
	//call recursive function
	return hullPixels;
}